
#include <stdint.h>
#include <stdarg.h>
#include <string.h>

#include "hardware/address_mapped.h"
#include "hardware/uart.h"


/*
UART0, 0x40034000
UART1, 0x40038000
*/
static void  dbg_uart_puts( const char * ptr, int tlen )
{
    uart_write_blocking( uart0, (const uint8_t *)ptr, tlen );
	return;
}


int putchar( int ch )
{
    uart_putc_raw( uart0, (char)ch );
    return ch;
}


int puts( const char * ptr )
{
    int  i;
    
    for ( i=0; ptr[i] != '\0' ; i++ )
    {
        putchar( ptr[i] );
    }
    
    putchar( 0x0A );
    return i;
}


static int  mini_itoa(int value, unsigned int radix, unsigned int uppercase, unsigned int unsig, char *buffer, int zero_pad )
{
	char * pbuffer = buffer;
	int	 negative = 0;
	int	 i, len;

	/* No support for unusual radixes. */
	if (radix > 16)
		return 0;

	if (value < 0 && !unsig) {
		negative = 1;
		value = -value;
	}

	/* This builds the string back to front ... */
	do {
		int digit = value % radix;
		*(pbuffer++) = (digit < 10 ? '0' + digit : (uppercase ? 'A' : 'a') + digit - 10);
		value /= radix;
	} while (value > 0);

	for (i = (pbuffer - buffer); i < zero_pad; i++)
		*(pbuffer++) = '0';

	if (negative)
		*(pbuffer++) = '-';

	*(pbuffer) = '\0';

	/* ... now we reverse it (could do it recursively but will
	 * conserve the stack space) */
	len = (pbuffer - buffer);
	for (i = 0; i < len / 2; i++) {
		char j = buffer[i];
		buffer[i] = buffer[len-i-1];
		buffer[len-i-1] = j;
	}

	return len;
}


int  mini_vprintf( const char * fmt, va_list vst )
{
    char  ch;
	int  tcnt = 0;
    char  tch;
    const char * ptr;
    int  ccnt;
    int  zero_pad;
    char  bf[28];

    while( 1 )
    {
        ch = *(fmt++);
        if ( ch == '\0' )
        {
            break;
        }

        /**/        
        if ( ch != '%' )
        {
            ptr = fmt - 1;
            ccnt = 1;
            tcnt += 1;

            /**/
            while ( 1 )
            {
                ch = *(fmt++);
                if ( ch == '\0' )
                {
                    /* flush */
                    dbg_uart_puts( ptr, ccnt );
                    goto  _final_rtn;
                }

                if ( ch == '%' )
                {
                    /* flush */
                    dbg_uart_puts( ptr, ccnt );
                    goto  _fmt_swi;
                }

                /**/
                ccnt += 1;
                tcnt += 1;
            }
        }
        else
        {
_fmt_swi:
            ptr = bf;
            zero_pad = 0;
            ch = *(fmt++);
            if ( ch == '\0' )
            {
                /* nothing */
                goto  _final_rtn;
            }

            if ( ch == '0' )
            {
                ch = *(fmt++);
                if ( ch == '\0' )
                {
                    /* nothing */
                    goto  _final_rtn;
                }

                if (ch >= '0' && ch <= '9')
                {
					zero_pad = ch - '0';

                    ch = *(fmt++);
                    if ( ch == '\0' )
                    {
                        /* nothing */
                        goto  _final_rtn;
                    }
                }
            }

            /**/
            switch ( ch )
            {
            case 'p':
            case 'x':
                ccnt = mini_itoa( va_arg(vst, unsigned int), 16, 0, 1, bf, zero_pad );
                break;

            case 'X':
                ccnt = mini_itoa( va_arg(vst, unsigned int), 16, 1, 1, bf, zero_pad );
                break;
            
            case 'd':
                ccnt = mini_itoa( va_arg(vst, int), 10, 0, 0, bf, zero_pad );
                break;

            case 'u':
                ccnt = mini_itoa( va_arg(vst, unsigned int), 10, 0, 1, bf, zero_pad );
                break;

            case 'c':
                bf[0] = va_arg( vst, int );
                ccnt = 1;
                break;

            case 's':
                ptr = va_arg( vst, char* );
                ccnt = strlen( ptr );
                break;

            default:
                bf[0] = ch;
                ccnt = 1;
            }

            /**/
            dbg_uart_puts( ptr, ccnt );
            tcnt += ccnt;
        }

    }

_final_rtn:
    return tcnt;
}


int  printf( const char * fmt, ... )
{
    int  iret;
    va_list  vst;

    /**/
	va_start(vst, fmt);
    iret = mini_vprintf( fmt, vst );
    va_end(vst);

    /**/
    return iret;
}


